#include <buola/io/cipendpoint.h>
#include <buola/io/csocket.h>
#include <buola/image/algorithm.h>
#include <buola/image/io.h>
#include <buola/image/algorithm/calibration.h>
#include <buola/app/ccmdline.h>
#include <buola/zz/operators.h>
#include <buola/zz/zinteger.h>
#include <buola/zz/zraw.h>
#include <buola/zz/zchar.h>
#include <buola/zz/zreal.h>
#include <buola/zz/zliteral.h>
#include <buola/algorithm/comparison.h>
#include <buola/cv/ccloud.h>

using namespace buola;

std::array<double,45> read_joints(const io::CURI &pFile)
{
    std::array<double,45> lJoints;
    
    io::CTextReader lS(pFile);
    std::string lStr;
    while(get_line(lS,lStr))
    {
        if(starts_with(lStr,"  position"))
        {
            std::tuple<int,double> lAttr;
            if(!zz::parse(lStr.begin(),lStr.end(),"  position["_z >> zz::int32()>> "]: "_z >> zz::dbl(),lAttr)) continue;
            msg_info() << std::get<0>(lAttr) << " " << std::get<1>(lAttr) << "\n";
            lJoints[std::get<0>(lAttr)]=std::get<1>(lAttr);
        }
    }

    return lJoints;
}

int main(int pNArgs,char **pArgs)
{
    buola_init(pNArgs,pArgs);

    try
    {
        img::CImage_rgb8 lRGBImage;
        img::CImage_gray32f lDepthImage;
        
        int lJointIndex;
        
        if(cmd_line().ArgCount()==3)
        {
            img::load(lRGBImage,io::arg(0));
            img::load_ascii(lDepthImage,io::arg(1),lRGBImage.Size());
            
            lJointIndex=2;
        }
        else if(cmd_line().ArgCount()==2)
        {
            img::load_rgbd(lRGBImage,lDepthImage,io::arg(0));

            lJointIndex=1;
        }
        else
        {
            throw XArgument("wrong number of arguments... specify URDF, RGB, depth and joints files");
        }

        img::CCameraParameters lParams;
        lParams.SetMatrix(CPoint_d(525,525),CPoint_d(320,240));
        img::CImage_xyz32f lPosImage=cvi::make_pos_image(lDepthImage,lParams);

        std::array<double,45> lJoints=read_joints(io::arg(lJointIndex));

        msg_info() << "read data!\n";

        auto lSocket=io::connect_to(io::CIPEndPoint("130.237.218.90",22333));

        lSocket->Send(lPosImage.RawBytes(),640*480*3*sizeof(float));
        lSocket->Send(lRGBImage.RawBytes(),640*480*3);
        lSocket->Send(lJoints.data(),45*sizeof(double));

        msg_info() << "sent data!\n";

        img::CImage_gray8 lTransformIndex({640,480});
        msg_info() << "received data! " << lSocket->Receive(lTransformIndex.RawBytes(),640*480,MSG_WAITALL) << "\n";
        img::save(lTransformIndex,io::file("transforms.pgm"));
        
    }
    catch(std::exception &pE)
    {
        msg_error() << pE.what() << "\n";
    }

    return buola_finish();
}
